home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / demos / audio / drive / Dynamics.c++ < prev    next >
Encoding:
C/C++ Source or Header  |  1994-08-02  |  21.6 KB  |  794 lines
  1. /*
  2.  * Copyright 1992-1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. //////////////////////////////////////////////////////////////////////
  18. // Dynamics.c++ - implementation of the dynamics class
  19. //////////////////////////////////////////////////////////////////////
  20.  
  21. #include "Defines.h"
  22. #include "Simulation.h"
  23. #include "MiscMath.h"
  24. #include "Dynamics.h"
  25. #include "Car.h"
  26. #include "Driver.h"
  27. #include "Cockpit.h"
  28. #include "Engine.h"
  29. #include "Road.h"
  30. #include "Stretch.h"
  31. #include "Noise.h"
  32. #include "Timer.h"
  33. #include "Bspline.h"
  34. #include <Inventor/SbString.h>
  35.  
  36. int Dynamics::_num_robots = 0;
  37.  
  38. Dynamics::Dynamics(Car *c)
  39. {
  40.     _car = c;
  41.  
  42.     // gravity in road units per second per second
  43.     _gravity = 32.0;
  44.  
  45.     // start the car in the right place
  46.     if (_car->get_type() == LOCAL_CAR)
  47.         reset_to_start(_car->get_road()->get_start(), RIGHT);
  48.     else
  49.     {
  50.         // spread robot cars around the track
  51.         const Stretch * start = _car->get_road()->get_start();
  52.         
  53.         for (int i = 0; i < _num_robots+1; i++)
  54.         {
  55.             if (_num_robots%2)
  56.                 start = start->get_next();
  57.             else
  58.                 start = start->get_prev();
  59.         }
  60.  
  61.         reset_to_start(start, RIGHT);
  62.         _num_robots++;
  63.     }
  64.  
  65.     // Screech noise starts as discontinuous, is made continuous when
  66.     // the tires are squeeling, then made discontinuous again.
  67.     // Assume longest length of 8 fps.
  68.     // XXX For now, only the local car makes noise. Soon, you'll
  69.     // be able to hear other car's on the track as well.
  70.     _screech = NULL;
  71.     if ((_car->get_type() == LOCAL_CAR) && 
  72.         _car->get_simulation()->sound_capable())
  73.     {
  74.         SbString screechpath(_car->get_simulation()->get_path().getString());
  75.         SbString screechfile("screech.aiff");
  76.         screechpath += screechfile;
  77.         
  78.         _screech = new Noise(screechpath.getString(),125000,FALSE);
  79.     }
  80. }
  81.  
  82.  
  83. // Moves the car to the beginning of the givin stretch, orients it
  84. // in the right direction, and puts it at the middle of the
  85. // given side.
  86. //
  87. // If side is RIGHT, the car's back starts at marker 0, and faces in
  88. // the direction of increasing markers.
  89. //
  90. // If side is LEFT, the car's back starts at marker n-1, and faces in
  91. // the direction of decreasing markers.
  92. //
  93. // XXX Only RIGHT supported, right now.
  94. void Dynamics::reset_to_start(const Stretch *start, int side)
  95. {
  96.     // initial dynamics
  97.     _velocity = 0.0;
  98.  
  99.     // initialize back position stuff
  100.     if (side == RIGHT)
  101.     {
  102.         _stretch[BACK] = (Stretch *)start;
  103.         _marker[BACK] = 0;
  104.         _marker_offset[BACK] = 0.0;
  105.  
  106.         // position back at middle of right side of road
  107.         // XXX different for english driving
  108.         _position[BACK] = start->road_pos(RIGHT,0);
  109.     }
  110.     else
  111.         fprintf(stderr,"Dynamics::reset_to_start - LEFT not yet    supported\n");
  112.         
  113.     // set the front stretch, marker, and position
  114.     // based on the wheelbase of the car
  115.     initialize_front(side);
  116.     
  117.     _center_position = (_position[BACK] + _position[FRONT])/2.0;
  118.  
  119.     // we're certainly on the road
  120.     _off_road_left = _off_road_right = FALSE;
  121.     
  122.     // initial orientation based on actual position on road
  123.     compute_orientation();
  124.  
  125.     // set eye position based on orientation
  126.     update_eye_position();
  127.  
  128.     compute_side_positions();
  129.  
  130.     _stretch[BACK]->add_car(_car);
  131. }
  132.  
  133.  
  134. Dynamics::~Dynamics()
  135. {
  136.     if (_screech)
  137.         delete _screech;
  138. }
  139.  
  140.  
  141. // Sets the front stretch, marker and position based on
  142. // the car's wheelbase and the back stretch.
  143. // Assumes that the back marker and marker offset are 0.
  144. void Dynamics::initialize_front(int side)
  145. {
  146.     // XXX
  147.     if (side == LEFT)
  148.     fprintf(stderr,"Dynamics::initialize_front - LEFT not yet supported\n");
  149.     
  150.     float total_length = 0.0;
  151.     float segment_length;
  152.     Stretch *stretch = _stretch[BACK];
  153.     int marker = 0;
  154.     
  155.     while (total_length < _car->get_wheelbase())
  156.     {
  157.         segment_length = stretch->get_direction(marker, MIDDLE).length();
  158.         total_length += segment_length;
  159.  
  160.         if (total_length <= _car->get_wheelbase())
  161.         {
  162.             int next_marker;
  163.             const Stretch *next_stretch = 
  164.                 stretch->get_next_stretch(marker, next_marker);
  165.  
  166.             stretch = (Stretch *)next_stretch;
  167.             marker = next_marker;
  168.         }
  169.     }
  170.  
  171.     _stretch[FRONT] = stretch;
  172.     _marker[FRONT] = marker;
  173.     
  174.     _marker_offset[FRONT] = 
  175.     (segment_length - (total_length - _car->get_wheelbase()))/segment_length;
  176.  
  177.     int next_marker;
  178.     const Stretch *next_stretch = 
  179.         _stretch[FRONT]->get_next_stretch(_marker[FRONT], next_marker);
  180.         
  181.     SbVec3f this_stretch_pos = _stretch[FRONT]->road_pos(RIGHT,_marker[FRONT]);
  182.     SbVec3f next_stretch_pos = next_stretch->road_pos(RIGHT,next_marker);
  183.         
  184.     _position[FRONT] = 
  185.         (1.0-_marker_offset[FRONT])*this_stretch_pos +
  186.         _marker_offset[FRONT]*next_stretch_pos;
  187. }
  188.  
  189.  
  190. // Knowing the back and front positions, the up vector (_pitch_roll),
  191. // and the width of the car, compute the left and right positions.
  192. // The left and right positions are at the front of the car.
  193. void Dynamics::compute_side_positions()
  194. {
  195.     SbVec3f forward = _position[FRONT] - _position[BACK];
  196.     SbVec3f right = forward.cross(_pitch_roll);
  197.     right.normalize();
  198.  
  199.     SbVec3f offset = (_car->get_width()/2.0)*right;
  200.     _side_position[RIGHT] = _position[FRONT] + offset;
  201.     _side_position[LEFT] = _position[FRONT] - offset;
  202. }
  203.  
  204.  
  205.  
  206. // returns radians change in yaw based on steering wheel
  207. float Dynamics::get_delta_yaw() const
  208. {
  209.     return 
  210.         _car->get_driver()->get_cockpit()->get_steering()*
  211.         _car->get_engine()->get_yaw_per_steering();
  212. }
  213.  
  214.  
  215. // Returns the normal representing pitch and roll for the car.
  216. // Based on normals at current and next marker positions,
  217. // interpolated by the marker offset.
  218. //
  219. // XXX Also sets the left and right flags that show the car's
  220. // position along the side of the road.
  221. SbVec3f Dynamics::get_pitch_roll() const
  222. {
  223.     SbVec3f interp[2];
  224.  
  225.     for (int i = BACK; i <= FRONT; i++)
  226.     {
  227.         SbVec3f normal[2], flag[2][2];
  228.         const Stretch * next_stretch;
  229.         int next_marker;
  230.  
  231.         next_stretch = _stretch[i]->get_next_stretch(_marker[i], next_marker);
  232.  
  233.         normal[0] = _stretch[i]->get_normal(_marker[i]);
  234.         normal[1] = next_stretch->get_normal(next_marker);
  235.  
  236.         flag[LEFT][0] = _stretch[i]->get_left(_marker[i]);
  237.         flag[LEFT][1] = next_stretch->get_left(next_marker);
  238.         
  239.         flag[RIGHT][0] = _stretch[i]->get_right(_marker[i]);
  240.         flag[RIGHT][1] = next_stretch->get_right(next_marker);
  241.     
  242.         // interpolate normal based on marker offset
  243.         interp[i] = 
  244.             (1.0 - _marker_offset[i])*normal[0] + 
  245.             _marker_offset[i]*normal[1];
  246.  
  247.         // interpolate side-of-road position
  248. //         _flag[LEFT][i] = 
  249.          flag[LEFT][i] = 
  250.             (1.0 - _marker_offset[i])*flag[LEFT][0] + 
  251.             _marker_offset[i]*flag[LEFT][1];
  252.             
  253. //         _flag[RIGHT][i] = 
  254.          flag[RIGHT][i] = 
  255.             (1.0 - _marker_offset[i])*flag[RIGHT][0] + 
  256.             _marker_offset[i]*flag[RIGHT][1];
  257.     }
  258.  
  259.     SbVec3f result = (interp[BACK] + interp[FRONT])/2.0;
  260.     result.normalize();
  261.  
  262.     return result;
  263. }
  264.  
  265.  
  266.  
  267. void Dynamics::update_marker(int end)
  268. {
  269.     // find the next stretch and marker. Most of the time,
  270.     // next_stretch will == _stretch[end] (unless the car
  271.     // really has moved on to the next stretch
  272.     int next_marker;
  273.     const Stretch * next_stretch = 
  274.         _stretch[end]->get_next_stretch(_marker[end],next_marker);
  275.  
  276.     /// keep the wheels on the ground by projecting the
  277.     /// wheels onto the ground plane
  278.  
  279.     // Construct the road plane from three points on the road
  280.     SbPlane road_plane(
  281.         _stretch[end]->get_left(_marker[end]),
  282.         _stretch[end]->get_right(_marker[end]),
  283.         next_stretch->get_left(next_marker));
  284.  
  285.     // Construct line passing through wheel with normal of car
  286.     SbLine wheel_line(_position[end], _position[end] + _pitch_roll);
  287.  
  288.     // intersection of that line with the road plane is the
  289.     // projection of the wheels onto the road
  290.     if (! road_plane.intersect(wheel_line,_position[end]))
  291.         fprintf(stderr,"Dynamics: wheel_line parallel to road!\n");
  292.  
  293.  
  294.     /// Update the stretch, marker and marker offset.
  295.  
  296.     // find the marker offset
  297.     // marker offset is 0.0 at the beginning of a stretch segment,
  298.     // 1.0 when you hit the next marker (at which point it rolls
  299.     // over to 0.0 again)
  300.     float new_marker_offset = 0.0;
  301.     
  302. //printf("update1: new_marker_offset %f, this %x\n",new_marker_offset,this);
  303.  
  304.     // XXX had to do this to avoid NaN's
  305.     Boolean on_to_next_marker;
  306.     on_to_next_marker = _stretch[end]->get_marker_offset(
  307.             _marker[end],_position[end], &new_marker_offset);
  308.             
  309. //printf("update2: new_marker_offset %f\n",new_marker_offset);
  310. //if (new_marker_offset != new_marker_offset)
  311. //printf("HOSED1\n");
  312. //if ((new_marker_offset < 0.0) || (new_marker_offset > 1.0))
  313. //printf("HOSED2\n");
  314.  
  315.     if (on_to_next_marker)
  316.     {
  317.         // the stretches maintain a list of which cars
  318.         // are on them. If we went on to the next stretch,
  319.         // remove the car from this one and add it to the next.
  320.         if ((end == BACK) && (next_stretch != _stretch[end]))
  321.         {
  322.             _stretch[end]->remove_car(_car);
  323.             ((Stretch *)next_stretch)->add_car(_car);
  324.         }
  325.         
  326.         _stretch[end] = (Stretch *) next_stretch;
  327.         _marker[end] = next_marker;
  328.  
  329.         // XXX is this the right place to do this?
  330.         // check if lap
  331.         if ((end == FRONT) && 
  332.             (_stretch[end] == _car->get_road()->get_start()) &&
  333.             (_marker[end] == 0) &&
  334.             (_car->get_type() == LOCAL_CAR))
  335.                 _car->get_driver()->get_timer()->lap();
  336.     }
  337.  
  338.     _marker_offset[end] = new_marker_offset;
  339. //printf("update3: new_marker_offset %f\n",_marker_offset[end]);
  340. }
  341.  
  342.  
  343.  
  344.  
  345. // Update the car's position, stretches, markers and marker offsets.
  346. // Returns the vector the car moved
  347. SbVec3f Dynamics::update_position()
  348. {
  349.     //// First, update the car's position based on its velocity and
  350.     //// orientation. Do this by pumping a velocity vector through
  351.     //// the orientation matrix to find the offset (from this current
  352.     //// position to next position. Then add that offset to the
  353.     //// back position. Then update the front position relative
  354.     //// to the back position.
  355.  
  356.     // distance the car moved in this frame
  357.     // dist/frame = dist/hour * hour/60min * min/60sec * sec/frame
  358.     SbVec3f frame_velocity(0,0,0);
  359.     frame_velocity[2] = 
  360.         - _car->get_road()->get_distance_factor() * 
  361.         _velocity/(60.0*60.0*_car->get_simulation()->get_fps());
  362.     
  363.     // get the orientation rotation matrix
  364.     SbMatrix orientation;
  365.     _motion_orientation.getValue(orientation);
  366.  
  367.     // rotate the velocity vector to get the offset
  368.     SbVec3f offset;
  369.     orientation.multVecMatrix(frame_velocity, offset);
  370.  
  371.     // XXX not quite sure why this is necessary, coordinate systems maybe?
  372.     offset[0] = - offset[0];    
  373.     float offset_length = offset.length();
  374.  
  375.     // XXX Ick. Need to handle long offsets
  376.     if (offset_length > _stretch[FRONT]->get_step())
  377.     {
  378.         float new_offset_length = _stretch[FRONT]->get_step();
  379.         offset /= (offset_length/new_offset_length);
  380.     }
  381.     
  382.     //// Now that we have the car's offset, update the position of the
  383.     //// car, making sure it does not penetrate the road. Then update
  384.     //// the stretch, marker and marker offset.
  385.     //// Do all this for both the back and the front of the car.
  386.  
  387.     // update the position of the car
  388.     _position[BACK] += offset;
  389.     update_marker(BACK);
  390.  
  391.     // Front of the car is extended forwards in the
  392.     // direction of the offset by the length of the car
  393.     SbVec3f front_offset = offset;
  394.     front_offset.normalize(); // make unit length
  395.     front_offset *= _car->get_wheelbase();
  396.  
  397.     // don't do anything if the offset is too small
  398.     if (fabs(front_offset.length() - _car->get_wheelbase()) < .01)
  399.     {
  400.         _position[FRONT] = _position[BACK] + front_offset;
  401.         update_marker(FRONT);
  402.     }
  403.  
  404.     // update the center position of the car
  405.     _center_position = (_position[BACK] + _position[FRONT])/2.0;
  406.     
  407.     /// Check to see if we went off the road
  408.     
  409.     SbVec3f left_vec = _stretch[FRONT]->get_direction(_marker[FRONT],LEFT);
  410.     float left_dist = 
  411.         point_vec_distance(_position[FRONT], left_vec, 
  412.                            _stretch[FRONT]->get_left(_marker[FRONT]));
  413.  
  414.     if (! _off_road_left)
  415.         _off_road_right = 
  416.         ((left_dist + _car->get_width()/2.0) > _stretch[FRONT]->get_width());
  417.  
  418.         
  419.     SbVec3f right_vec = _stretch[FRONT]->get_direction(_marker[FRONT],RIGHT);
  420.     float right_dist = 
  421.         point_vec_distance(_position[FRONT], right_vec,
  422.                            _stretch[FRONT]->get_right(_marker[FRONT]));
  423.  
  424.     if (! _off_road_right)
  425.         _off_road_left = 
  426.         ((right_dist + _car->get_width()/2.0) > _stretch[FRONT]->get_width());
  427.  
  428.     /// return the car's positional offset vector
  429.     return offset;
  430. }
  431.  
  432. void Dynamics::update_eye_position()
  433. {
  434.     // update the eye position (relative to car) based on pitch and roll
  435.     _pitch_roll_mat.multDirMatrix(_car->get_driver_position(),_eye_position);
  436. }
  437.  
  438.  
  439. // Update the a robot car's position, stretches, markers and marker offsets.
  440. void Dynamics::update_robot_position()
  441. {
  442. //printf("\nrobot dynamics %x\n",this);
  443.  
  444.     // find the middle of the right side for the previous, current
  445.     // and next markers
  446.     
  447.     SbVec3f this_stretch_pos = _stretch[BACK]->road_pos(RIGHT,_marker[BACK]);
  448.     
  449.     int next_marker;
  450.     const Stretch *next_stretch = 
  451.         _stretch[BACK]->get_next_stretch(_marker[BACK], next_marker);
  452.     SbVec3f next_stretch_pos = next_stretch->road_pos(RIGHT,next_marker);
  453.     
  454.     int prev_marker;
  455.     const Stretch *prev_stretch = 
  456.         _stretch[BACK]->get_prev_stretch(_marker[BACK], prev_marker);
  457.     SbVec3f prev_stretch_pos = prev_stretch->road_pos(RIGHT,prev_marker);
  458.          
  459.     // offset the car moved in this frame
  460.     // offset/frame = (dist/frame)*(offset/dist)
  461.     // dist/frame = dist/hour * hour/60min * min/60sec * sec/frame
  462.     float offset = 
  463.         _marker_offset[BACK] +
  464.         (1.0/_stretch[BACK]->get_step()) *
  465.         _car->get_road()->get_distance_factor() * 
  466.         _velocity/(60.0*60.0*_car->get_simulation()->get_fps());
  467.  
  468. /*        
  469. printf("marker_offset before %f\n", _marker_offset[BACK]);
  470. printf("marker before %d\n", _marker[BACK]);
  471. printf("offset before %f\n", offset);
  472. */
  473.  
  474.     // if we go on to the next marker, update the stretch and markers
  475.     if (offset >= 1.0)
  476.     {
  477.         offset -= 1.0;
  478.         prev_stretch_pos = this_stretch_pos;
  479.         this_stretch_pos = next_stretch_pos;
  480.  
  481.         prev_stretch = _stretch[BACK];
  482.         prev_marker = _marker[BACK];
  483.  
  484.         if (next_stretch != _stretch[BACK])
  485.         {
  486.             _stretch[BACK]->remove_car(_car);
  487.             ((Stretch *)next_stretch)->add_car(_car);
  488.         }
  489.         
  490.         _stretch[BACK] = (Stretch *)next_stretch;
  491.         _marker[BACK] = next_marker;
  492.         _marker_offset[BACK] = 0.0; // XXX shouldn't need this
  493.         // but am getting NaN's for some reason after running
  494.         // for a long time
  495.         
  496.         next_stretch = 
  497.         _stretch[BACK]->get_next_stretch(_marker[BACK], next_marker);
  498.  
  499.         next_stretch_pos = next_stretch->road_pos(RIGHT,next_marker);
  500.     }
  501.  
  502. /*    
  503. printf("offset after %f\n", offset);
  504. printf("marker after %d\n", _marker[BACK]);
  505. printf("marker_offset after %f\n", _marker_offset[BACK]);
  506. */    
  507.  
  508.     // turn offset into parametric value where 0.0 would be
  509.     // at previous marker, .5 at this marker and 1.0 at next
  510.     // marker. The offset the program uses goes from 0.0
  511.     // to 1.0 from this marker to the next marker only.
  512.     float u = offset/2.0 + 0.5;
  513.  
  514.     // interpolate the back of the car along a quadratic bspline
  515.     // whose control points are the middle of the right side of
  516.     // the road at each marker
  517.  
  518.     // basis functions
  519.     float b0 = bsbasis(2,0,u);
  520.     float b1 = bsbasis(2,1,u);
  521.     float b2 = bsbasis(2,2,u);
  522.     
  523.     _position[BACK] = 
  524.         b0*prev_stretch_pos +
  525.         b1*this_stretch_pos +
  526.         b2*next_stretch_pos;
  527.  
  528.     update_marker(BACK);
  529.  
  530. /*    
  531. printf("marker way after %d\n", _marker[BACK]);
  532. printf("marker_offset way after %f\n", _marker_offset[BACK]);
  533.  
  534. printf("u %f\n", u);
  535. printf("basis %f %f %f, total %f\n",b0,b1,b2,b0+b1+b2);
  536.  
  537. printf("a = "); print_vec(prev_stretch_pos);
  538. printf("b = "); print_vec(this_stretch_pos);
  539. printf("c = "); print_vec(next_stretch_pos);
  540. printf("back = "); print_vec(_position[BACK]);
  541. */
  542.  
  543.     // orient the car by interpolating the road directions
  544.     // at the 3 markers using the same basis functions
  545.     SbVec3f prev_stretch_dir = 
  546.         prev_stretch->get_direction(prev_marker, MIDDLE);
  547.     SbVec3f this_stretch_dir = 
  548.         _stretch[BACK]->get_direction(_marker[BACK], MIDDLE);
  549.     SbVec3f next_stretch_dir = 
  550.         next_stretch->get_direction(next_marker, MIDDLE);
  551.     SbVec3f front_offset = 
  552.         b0*prev_stretch_dir +
  553.         b1*this_stretch_dir +
  554.         b2*next_stretch_dir;
  555.     
  556.     front_offset.normalize(); // make unit length
  557.     front_offset *= _car->get_wheelbase();
  558.  
  559.     _position[FRONT] = _position[BACK] + front_offset;
  560.     update_marker(FRONT);
  561.     
  562. //printf("front = "); print_vec(_position[FRONT]);
  563.  
  564.  
  565.     _center_position = (_position[FRONT] + _position[BACK])/2.0;
  566. }
  567.  
  568.  
  569. // computes car's orientation based on the front and back positions
  570. void Dynamics::compute_orientation()
  571. {
  572.     SbVec3f forward = _position[FRONT] - _position[BACK];
  573.  
  574.     // compute only in ground plane
  575.     forward[1] = 0.0;
  576.     
  577.     SbVec3f north(0.0,0.0,-1.0);
  578.  
  579.     // check cross product to determine orientation
  580.     if (north.cross(forward)[1] < 0.0)
  581.         _yaw = angle_between(north, forward);
  582.     else
  583.         _yaw = -angle_between(north, forward);
  584.         
  585.     compute_pitch_roll();
  586. }
  587.  
  588.  
  589.  
  590. // updates the orientation of the car based on previous yaw and
  591. // new steering
  592. void Dynamics::update_orientation()
  593. {
  594.     /// _yaw is in radians. 0.0 is pointing north (0,0,-1).
  595.     /// negative yaw toward west, positive towards east
  596.     
  597.     // new radians of yaw, based on steering
  598.     // XXX what's the 20.0 for?
  599.     _yaw += 20.0*get_delta_yaw()/_car->get_simulation()->get_fps();
  600.  
  601.     if (_yaw >= 2.0*M_PI)
  602.         _yaw -= 2.0*M_PI;
  603.     else if (_yaw <= -2.0*M_PI)
  604.         _yaw += 2.0*M_PI;
  605.  
  606.     compute_pitch_roll();
  607. }
  608.  
  609.     
  610. void Dynamics::compute_pitch_roll()
  611. {
  612.     const SbVec3f up(0,1,0);
  613.     
  614.     // yaw is a rotation about the car's up vector
  615.     SbRotation yaw_quat(up,_yaw);
  616.     SbRotation minus_yaw_quat(up,-_yaw);
  617.  
  618.     // get normal representing pitch and roll based on position on road
  619.     // this is a vector which is the 'normal' of the car
  620.     _pitch_roll = get_pitch_roll();
  621.  
  622.     // Rotation used to move the car. 
  623.     // The rotation needed to get from up to pitch_roll.
  624.     SbRotation motion_pitch_roll_quat(up, _pitch_roll);
  625.     _motion_orientation = yaw_quat*motion_pitch_roll_quat;
  626.  
  627.     // Rotation used to set the view. The view is set by
  628.     // rotating the world, so this from pitch_roll to up.
  629.     SbRotation view_pitch_roll_quat(_pitch_roll, up);
  630.     _view_orientation = view_pitch_roll_quat*yaw_quat;
  631.  
  632.     // this is used to set the eye position in world coords
  633.     motion_pitch_roll_quat.getValue(_pitch_roll_mat);
  634. }
  635.  
  636.  
  637. void Dynamics::update_velocity()
  638. {
  639.     float friction, gravity;
  640.     float fps = _car->get_simulation()->get_fps();
  641.     
  642.     // update velocity based on rpms if not in neutral
  643.     // dist/hour = engine-revs/min * 60min/hour * 
  644.     //             axel-revs/engine-revs * dist/axel-rev
  645.     if (_car->get_engine()->get_gear() != 0)
  646.     {
  647.         _velocity =  
  648.             (float)_car->get_engine()->get_rpm() * 60.0 * 
  649.             _car->get_engine()->get_ratio() *
  650.             _car->get_engine()->get_tire_diam() * 
  651.             (1.0/_car->get_road()->get_distance_factor()) * M_PI;
  652.     }
  653.     else
  654.     {
  655.         // subtract tire friction
  656.         friction = 2.0/fps;
  657.         _velocity -= friction;
  658.     }
  659.     
  660.     // effect of gravity
  661.     float tilt = _position[BACK][1] - _position[FRONT][1];
  662.     float incline = tilt/_car->get_wheelbase();
  663.         
  664.     // dist/hour = dist/s^2 * 60s/hour * s;
  665.     gravity = 40.0*(_gravity/5280.0)*incline*60.0/fps;
  666.     _velocity += gravity;
  667.         
  668.     // subtract braking
  669.     // dist/hour = dist-lost/sec *
  670.     _velocity -= _car->get_driver()->get_cockpit()->get_brakes();
  671.  
  672.     // Make offroad be like a swamp. 
  673.     // The longer off road, the more effect on
  674.     // velocity and rpm.
  675.     if (_off_road_left || _off_road_right)
  676.         _velocity *= .92; // XXX Hard-coding a value. Tsk Tsk.
  677. }
  678.  
  679.  
  680. void Dynamics::update_brakes()
  681. {
  682.     static float last_brakes = 0.0;
  683.     float this_brakes = _car->get_driver()->get_cockpit()->get_brakes();
  684.     Boolean brakes_applied = ((last_brakes == 0.0)&&(this_brakes == 1.0));
  685.     Boolean brakes_released = ((last_brakes == 1.0)&&(this_brakes == 0.0));
  686.     static Boolean screeching = FALSE;
  687.     
  688.     // XXX Ick. Need some _real_ dynamics.
  689.     if ((_velocity > 40.0) && brakes_applied)
  690.     {
  691.         if (_car->get_simulation()->play_sound())
  692.         {
  693. /*            
  694.             // XXX 
  695.             //something is amiss when using a second continuous audio process
  696.                 _screech->setContinuity(TRUE);
  697.                 _screech->play(1.0, _car->get_driver()->last_frame_time());
  698. */
  699.             screeching = TRUE;
  700.         }
  701.     }
  702.     else if (screeching &&
  703.         ((_velocity < 40.0) || (brakes_released)))
  704.     {
  705.         // _screech->setContinuity(FALSE);
  706.         screeching = FALSE;
  707.     }
  708.  
  709.     if (screeching && _car->get_simulation()->play_sound())
  710.         _screech->play(1.0, 125000);
  711.             
  712.     last_brakes = this_brakes;
  713. }
  714.  
  715.  
  716. // this is disgusting
  717. void Dynamics::check_collisions(SbVec3f offset)
  718. {
  719.     SbVec3f obstacle;
  720.     Boolean near_obstacle = FALSE;
  721.  
  722.     // yes, the position of the obstacles is hard-coded
  723.     int c1 = _stretch[FRONT]->get_count()/3;
  724.     int c2 = 2*_stretch[FRONT]->get_count()/3;
  725.     
  726.     if (_marker[FRONT] == 0)
  727.     {
  728.         obstacle = _stretch[FRONT]->get_pole_pos();
  729.         near_obstacle = TRUE;
  730.     }
  731.     else if (_marker[FRONT] == c1)
  732.     {
  733.         obstacle = _stretch[FRONT]->get_tree1_pos();
  734.         near_obstacle = TRUE;
  735.     }
  736.     else if (_marker[FRONT] == c2)
  737.     {
  738.         obstacle = _stretch[FRONT]->get_tree2_pos();
  739.         near_obstacle = TRUE;
  740.     }
  741.     
  742.  
  743.     if (near_obstacle)
  744.     {
  745.         compute_side_positions();
  746.         
  747.         SbVec3f car_rect[4];
  748.  
  749.         car_rect[0] = _side_position[LEFT] - offset;
  750.         car_rect[1] = _side_position[RIGHT] - offset;
  751.         car_rect[2] = _side_position[RIGHT];
  752.         car_rect[3] = _side_position[LEFT];
  753.         
  754.         if (point_within(car_rect,4,obstacle))
  755.         {
  756.             _car->get_driver()->crash();
  757.             _velocity = 0.0;
  758.         }
  759.     }
  760. }
  761.  
  762.  
  763. // XXX DESPARATELY SEEKING REAL DYNAMICS
  764. void Dynamics::update()
  765. {
  766.     SbVec3f offset;
  767.  
  768.     if (_car->get_type() == LOCAL_CAR)
  769.         update_velocity();
  770.     else
  771.         _velocity = 80.0; // XXX for robot car
  772.  
  773.     // update orientation and position only if moving
  774.     if (moving())
  775.     {
  776.         if (_car->get_type() == LOCAL_CAR)
  777.         {
  778.             offset = update_position();
  779.             update_orientation();
  780.             update_eye_position();
  781.             update_brakes();
  782.             check_collisions(offset);
  783.         }
  784.         else
  785.         {
  786.             update_robot_position();
  787.             compute_orientation();
  788.         }
  789.     }
  790.     else
  791.         _velocity = 0.0;
  792. }
  793.  
  794.